
#include "EthercatWorker.h"

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <thread>
#include "ConfigData.h"
#include <queue>
#include <ethercat.h>
#include "MotionTaskQueue.h"
#include "RealTimeData.h"
#include "RobotDelta.h"


EthercatWorker::EthercatWorker()
    : isRun(false)
{
    cfg = ConfigData::instance();
	taskQueue = MotionTaskQueue::instance();
	realtimeData = RealTimeData::instance();
}

EthercatWorker::~EthercatWorker()
{

}

void EthercatWorker::run()
{
    isRun = true;
	RobotDelta rDelta;
	rDelta.SetDeltaPara(cfg->theta1, cfg->theta2, cfg->theta3, cfg->rMoving, cfg->rStill, cfg->lenActive, cfg->lenPassive, cfg->lenPC, cfg->angleOffset);
	
    while (isRun)
    {
        try
        {
			int current_position[4] = {0};
			uint16_t statusWord[4] = {0};
            for(int axis_idx = 1; axis_idx <= cfg->AxisCount; axis_idx++)
			{
				int size_i = sizeof(current_position[axis_idx - 1]);
				int size_j = sizeof(statusWord[axis_idx - 1]);
				ec_SDOread(axis_idx, 0x6064, 0x00, FALSE, &size_i, &current_position[axis_idx - 1], EC_TIMEOUTRXM);
				ec_SDOread(axis_idx, 0x6041, 0x00, FALSE, &size_j, &statusWord[axis_idx - 1], EC_TIMEOUTRXM);
			}
			realtimeData->currentData["State_X"] = statusWord[0];
			realtimeData->currentData["State_Y"] = statusWord[1];
			realtimeData->currentData["State_Z"] = statusWord[2];
			realtimeData->currentData["State_R"] = statusWord[3];
				
			if(cfg->RobotType == 1)
			{
				realtimeData->currentData["Pos_X"] = current_position[0];
				realtimeData->currentData["Pos_Y"] = current_position[1];
				realtimeData->currentData["Pos_Z"] = current_position[2];
				realtimeData->currentData["Pos_R"] = current_position[3];
			}
			else if(cfg->RobotType == 2)  // Delta
			{
				double angle_x = (double)current_position[0] / (double)cfg->AxisCoderValue_x * 360.0f  / cfg->decRatio;
				double angle_y = (double)current_position[1] / (double)cfg->AxisCoderValue_y * 360.0f  / cfg->decRatio;
				double angle_z = (double)current_position[2] / (double)cfg->AxisCoderValue_z * 360.0f  / cfg->decRatio;
				double angle_r = (double)current_position[3] / (double)cfg->AxisCoderValue_r * 360.0f  / cfg->decRatio;
				
				double posX, posY, posZ;
				rDelta.TransThetaToPos(angle_x, angle_y, angle_z, posX, posY, posZ);
				realtimeData->currentData["Pos_X"] = posX;
				realtimeData->currentData["Pos_Y"] = posY;
				realtimeData->currentData["Pos_Z"] = posZ;
				realtimeData->currentData["Pos_R"] = angle_r;
			}
			
			for(int i = 0; i < taskQueue->reqMotion.size(); i++)
			{
				Data_Request dataReq;
				Motion_Request mReq = taskQueue->reqMotion.front();
				taskQueue->reqMotion.pop();
				if(mReq.CmdStr == "MoveToPos")
				{
					realtimeData->currentData["MoveToPos"] = 0;
					if(cfg->RobotType == 1)
			        {
						uint32_t targetPos = mReq.targetX;
						ec_SDOwrite(1, 0x607A, 0x00, FALSE, sizeof(targetPos), &targetPos, EC_TIMEOUTRXM);
						targetPos = mReq.targetY;
			        	ec_SDOwrite(2, 0x607A, 0x00, FALSE, sizeof(targetPos), &targetPos, EC_TIMEOUTRXM);
						targetPos = mReq.targetZ;
						ec_SDOwrite(3, 0x607A, 0x00, FALSE, sizeof(targetPos), &targetPos, EC_TIMEOUTRXM);
			        }
			        else if(cfg->RobotType == 2)  // Delta
			        {
						uint8_t modePP = 1;  // PP 模式（轮廓位置模式）
						ec_SDOwrite(1, 0x6060, 0x00, FALSE, sizeof(modePP), &modePP, EC_TIMEOUTRXM);  // 设置模式为 PP 模式
						ec_SDOwrite(2, 0x6060, 0x00, FALSE, sizeof(modePP), &modePP, EC_TIMEOUTRXM);  // 设置模式为 PP 模式
						ec_SDOwrite(3, 0x6060, 0x00, FALSE, sizeof(modePP), &modePP, EC_TIMEOUTRXM);  // 设置模式为 PP 模式
						
						std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 延时 100 毫秒
						
						double targetTh1, targetTh2, targetTh3;
			        	rDelta.TransPosToTheta(mReq.targetX, mReq.targetY, mReq.targetZ, targetTh1, targetTh2, targetTh3);
						int32_t px = targetTh1 / 360.0f * (double)cfg->AxisCoderValue_x * cfg->decRatio;
						ec_SDOwrite(1, 0x607A, 0x00, FALSE, sizeof(px), &px, EC_TIMEOUTRXM);
						int32_t py = targetTh2 / 360.0f * (double)cfg->AxisCoderValue_y * cfg->decRatio;
						ec_SDOwrite(2, 0x607A, 0x00, FALSE, sizeof(py), &py, EC_TIMEOUTRXM);
						int32_t pz = targetTh3 / 360.0f * (double)cfg->AxisCoderValue_z * cfg->decRatio;
						ec_SDOwrite(3, 0x607A, 0x00, FALSE, sizeof(pz), &pz, EC_TIMEOUTRXM);
						
						std::cout << "Move To:<" << px << "," << py << "," << pz << ">" << std::endl;
						
						std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 延时 100 毫秒
						
						uint32_t profileVelocity = mReq.moveSpeed * cfg->decRatio;
						ec_SDOwrite(1, 0x6081, 0x00, FALSE, sizeof(profileVelocity), &profileVelocity, EC_TIMEOUTRXM);  // 设置轮廓速度
						ec_SDOwrite(2, 0x6081, 0x00, FALSE, sizeof(profileVelocity), &profileVelocity, EC_TIMEOUTRXM);  // 设置轮廓速度
						ec_SDOwrite(3, 0x6081, 0x00, FALSE, sizeof(profileVelocity), &profileVelocity, EC_TIMEOUTRXM);  // 设置轮廓速度
						
						std::cout << "Set Speed:<" << profileVelocity << ">" << std::endl;
						
						std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 延时 100 毫秒
						
						uint16_t controlWord = 0x0006;  // 准备使能
						ec_SDOwrite(1, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						ec_SDOwrite(2, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						ec_SDOwrite(3, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 延时 100 毫秒
						
						controlWord = 0x0007;  // 电机上使能
						ec_SDOwrite(1, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						ec_SDOwrite(2, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						ec_SDOwrite(3, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 延时 100 毫秒
						
						controlWord = 0x000F;  // 电机上使能
						ec_SDOwrite(1, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						ec_SDOwrite(2, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						ec_SDOwrite(3, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 延时 100 毫秒
						
						controlWord = 0x001F;  // 启动运动
						ec_SDOwrite(1, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						ec_SDOwrite(2, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						ec_SDOwrite(3, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
			        }
					
					realtimeData->currentData["MoveToPos"] = 1;
					dataReq.DataNameList.push_back("MoveToPos");					
				}
				else if(mReq.CmdStr == "AllZero")
				{
					realtimeData->currentData["AllZero"] = 0;
					for(int axis_idx = 1; axis_idx <= 3; axis_idx ++)
					{
						uint16_t controlWord = 0x0012;  // 回零模式 18
						ec_SDOwrite(axis_idx, 0x6098, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 延时 100 毫秒
						
						// 配置 回零 模式
						uint8_t modeHome = 6;  // 回零 模式
						ec_SDOwrite(axis_idx, 0x6060, 0x00, FALSE, sizeof(modeHome), &modeHome, EC_TIMEOUTRXM);  // 设置模式为 回零 模式
						std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 延时 100 毫秒
						
						uint32_t profileVelocity = mReq.moveSpeed;
						ec_SDOwrite(axis_idx, 0x6081, 0x00, FALSE, sizeof(profileVelocity), &profileVelocity, EC_TIMEOUTRXM);  // 设置轮廓速度
						std::this_thread::sleep_for(std::chrono::milliseconds(100));
						
						controlWord = 0x0006;  // 准备使能
						ec_SDOwrite(axis_idx, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 延时 100 毫秒
						
						controlWord = 0x0007;  // 电机上使能
						ec_SDOwrite(axis_idx, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 延时 100 毫秒
						
						controlWord = 0x000F;  // 电机上使能
						ec_SDOwrite(axis_idx, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
						std::this_thread::sleep_for(std::chrono::milliseconds(100));  // 延时 100 毫秒
						
						controlWord = 0x001F;  // 启动运动
						ec_SDOwrite(axis_idx, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
					}
					
					realtimeData->currentData["AllZero"] = 1;
					dataReq.DataNameList.push_back("AllZero");
				}
				
				dataReq.reqSocket = mReq.reqSocket;
				MotionTaskQueue::instance()->mtxReqData.lock();
				MotionTaskQueue::instance()->reqData.push(dataReq);
				MotionTaskQueue::instance()->mtxReqData.unlock();
			}

            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            

        }
        catch (std::exception& e)
        {
            std::cerr << "Exception: " << e.what() << std::endl;
        }
    }
}

void EthercatWorker::stop()
{
    isRun = false;
}

void EthercatWorker::init()
{
    // 初始化 EtherCAT 主站
    if (ec_init(cfg->EthercatPortName.c_str())) 
	{
        printf("EtherCAT initialization succeeded.\n");
    }
    else 
	{
        printf("EtherCAT initialization failed.\n");
        return ;
    }

    // 配置从站
    if (ec_config_init(FALSE) > 0) 
	{
        printf("EtherCAT configuration succeeded.\n");
    }
    else 
	{
        printf("EtherCAT configuration failed.\n");
        ec_close();
        return ;
    }
	
	// 等待所有从站进入 OP 状态
    ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
	
	// 配置 PP 模式
    uint8_t modePP = 1;  // PP 模式（轮廓位置模式）
	uint32_t profileVelocity = cfg->AxisSpeed;
	uint32_t profileAcceleration = cfg->AxisAcc;
	uint32_t profileDeceleration = cfg->AxisDec;
	uint32_t coderValue = 1;
	for(int axis_idx = 1; axis_idx <= cfg->AxisCount; axis_idx++)
    {
		ec_SDOwrite(axis_idx, 0x6060, 0x00, FALSE, sizeof(modePP), &modePP, EC_TIMEOUTRXM);  // 设置模式为 PP 模式
		std::this_thread::sleep_for(std::chrono::milliseconds(100));
		
        ec_SDOwrite(axis_idx, 0x6081, 0x00, FALSE, sizeof(profileVelocity), &profileVelocity, EC_TIMEOUTRXM);  // 设置轮廓速度
		std::this_thread::sleep_for(std::chrono::milliseconds(100));
		
        ec_SDOwrite(axis_idx, 0x6083, 0x00, FALSE, sizeof(profileAcceleration), &profileAcceleration, EC_TIMEOUTRXM);  // 设置轮廓加速度
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
	    
        ec_SDOwrite(axis_idx, 0x6084, 0x00, FALSE, sizeof(profileDeceleration), &profileDeceleration, EC_TIMEOUTRXM);  // 设置轮廓减速度
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
		
		// 使能电机 06 -> 07 -> 0F 汇川定义的上使能流程
        uint16_t controlWord = 0x0006;  // 准备使能
        ec_SDOwrite(axis_idx, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
		
		controlWord = 0x0007;  // 电机上使能
		ec_SDOwrite(axis_idx, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
		std::this_thread::sleep_for(std::chrono::milliseconds(100));
		
		controlWord = 0x000F;  // 电机上使能
		ec_SDOwrite(axis_idx, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
		std::this_thread::sleep_for(std::chrono::milliseconds(100));
		
		ec_SDOwrite(axis_idx, 0x6091, 0x00, FALSE, sizeof(coderValue), &coderValue, EC_TIMEOUTRXM);
		std::this_thread::sleep_for(std::chrono::milliseconds(100));
	}
	
	
}

